// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PathPlanningAndTrackingMetricsControllerStatus.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Navigation
{
    using System;

    using Microsoft.Robotics.FiniteStateMachine;
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Tracing;
    using Test.Robotics.Navigation.Metrics.PathPlanningAndTracking;

    /// <summary>
    /// Status of the metrics run.
    /// </summary>
    public class PathPlanningAndTrackingMetricsControllerStatus : IContext<PathPlanningAndTrackingMetricsControllerStatus.StatusInput, PathPlanningAndTrackingMetricsControllerStatus.StatusOutput>
    {
        /// <summary>
        /// Initializes a new instance of the PathPlanningAndTrackingMetricsControllerStatus class.
        /// </summary>
        /// <param name="waypoints">Waypoint targets.</param>
        public PathPlanningAndTrackingMetricsControllerStatus(Waypoint[] waypoints)
        {
            this.Input = new StatusInput();
            this.Output = new StatusOutput();
            this.Waypoints = waypoints;
            this.Input.NavMetrics = new PathPlanningAndTrackingMetrics(0, 0, TimeSpan.Zero, new Pose2D(), 0, 0, 0, 0);
            this.Input.DriveDirection = new DriveDirection(double.NaN, double.NaN, double.NaN);
            this.Output.Reset();
        }

        /// <summary>
        /// Gets or sets values coming from the outside world.
        /// </summary>
        public StatusInput Input { get; set; }

        /// <summary>
        /// Gets or sets values affecting the outside world.
        /// </summary>
        public StatusOutput Output { get; set; }

        /// <summary>
        /// Gets the set of target waypoints for this metrics run.
        /// </summary>
        public Waypoint[] Waypoints { get; private set; }

        /// <summary>
        /// Gets or sets the current target (index).
        /// </summary>
        public int TargetIndex { get; set; }

        /// <summary>
        /// Gets the current target waypoint.
        /// </summary>
        public Waypoint CurrentTarget
        {
            get
            {
                return this.Waypoints[this.TargetIndex];
            }
        }

        /// <summary>
        /// Euclidean distance to target point.
        /// </summary>
        /// <param name="mapResolution">Map resolution in meters.</param>
        /// <returns>Proximity in meters.</returns>
        public double ProximityToTarget(double mapResolution)
        {
            Pose2D current = this.Input.NavMetrics.CurrentPose;
            Pose2D target = this.Waypoints[this.TargetIndex].Pose;

            double rx = current.X * mapResolution;
            double ry = current.Y * mapResolution;

            double gx = target.X * mapResolution;
            double gy = target.Y * mapResolution;

            return Math.Sqrt(Math.Pow(gy - ry, 2) + Math.Pow(gx - rx, 2));
        }

        /// <summary>
        /// Stop drive command.
        /// </summary>
        /// <returns>Updated status.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus Stop()
        {
            this.Output.Drive = DriveDirection.Stop;
            return this;
        }

        /// <summary>
        /// Initiate turret scan.
        /// </summary>
        /// <returns>Updated status.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus InitiateTurretScan()
        {
            this.Output.InitiateTurretScan = true;
            return this.Stop().ZeroElapsed();
        }

        /// <summary>
        /// Set target waypoint.
        /// </summary>
        /// <param name="target">Target waypoint index.</param>
        /// <returns>Updated status.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus SetTarget(int target)
        {
            this.TargetIndex = target;
            this.Output.Target = this.Waypoints[target].Pose;
            this.Output.WaypointId = target;
            return this;
        }

        /// <summary>
        /// Record navigation metrics.
        /// </summary>
        /// <returns>Updated status.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus RecordNavMetrics()
        {
            this.Output.NavMetrics = this.Input.NavMetrics;
            this.Output.RecordNavMetrics = true;
            return this;
        }

        /// <summary>
        /// Record localization metrics.
        /// </summary>
        /// <returns>Updated status.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus RecordLocMetrics()
        {
            this.Output.NavMetrics = this.Input.NavMetrics;
            this.Output.LocRelativePose = this.Input.LocRelativePose;
            this.Output.RecordLocMetrics = true;
            return this;
        }

        /// <summary>
        /// Record undocking as a localization metric.
        /// </summary>
        /// <returns>Updated status.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus RecordUndocking()
        {
            this.Output.WaypointId = StatusOutput.UndockingSpecialWaypointId;
            this.Output.RecordLocMetrics = true;
            return this;
        }

        /// <summary>
        /// Record docking as a localization metric.
        /// </summary>
        /// <returns>Updated status.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus RecordDockingAttempt()
        {
            this.Output.DockingAttempts++;
            this.Output.WaypointId = StatusOutput.DockingSpecialWaypointId;
            this.Output.RecordLocMetrics = true;
            return this;
        }

        /// <summary>
        /// Sets the current battery state information.
        /// </summary>
        /// <param name="charging">Whether the battery is charging.</param>
        /// <param name="percentage">Battery charge percentage.</param>
        /// <returns>Updated status.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus SetBatteryState(bool charging, double percentage)
        {
            this.Input.BatteryCharging = charging;
            this.Input.BatteryPercentage = percentage;
            return this;
        }

        /// <summary>
        /// Accumulates distance travelled since the last update assuming straight line or arc motion
        /// </summary>
        /// <param name="driveFeedbackState">Drive feedback state</param>
        /// <returns>Updated status.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus SetDistance(DriveFeedbackState driveFeedbackState)
        {
            if (this.Input.PreviousDriveFeedbackState == null)
            {
                this.Input.Distance = 0;
                this.Input.PreviousDriveFeedbackState = driveFeedbackState;
                return this;
            }

            this.Input.Distance += driveFeedbackState.GetMeanDeltaPose(this.Input.PreviousDriveFeedbackState).TotalTranslation();
            this.Input.PreviousDriveFeedbackState = driveFeedbackState;
            return this;
        }

        /// <summary>
        /// Reset the distance accumulator
        /// </summary>
        /// <returns>Updated status.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus ResetDistace()
        {
            this.Input.Distance = 0;
            this.Input.PreviousDriveFeedbackState = null;
            return this;
        }

        /// <summary>
        /// Set current localization ground truth measurement.
        /// </summary>
        /// <param name="relativePose">Robot pose relative to the landmark.</param>
        /// <returns>Updated status.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus SetLocalizationGroundTruth(Pose2D relativePose)
        {
            TraceOut.Info(TraceContexts.Nav, "Ground truth measurement: {0} (previous {1})", relativePose, this.Input.LocRelativePose);
            this.Input.LocalizationGroundTruthSamples++;
            this.Input.PreviousLocRelativePose = this.Input.LocRelativePose;
            this.Input.LocRelativePose = relativePose;
            return this;
        }

        /// <summary>
        /// Reset ground truth samples.
        /// </summary>
        /// <returns>Updated status.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus ResetGroundTruthSamples()
        {
            this.Input.LocalizationGroundTruthSamples = 0;
            this.Input.PreviousLocRelativePose =
            this.Input.LocRelativePose =
            new Pose2D(double.NaN, double.NaN, double.NaN);
            return this;
        }

        /// <summary>
        /// Set elapsed time to zero.
        /// </summary>
        /// <returns>Updated status.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus ZeroElapsed()
        {
            this.Input.Start = this.Input.Now;
            return this;
        }

        /// <summary>
        /// Set current time.
        /// </summary>
        /// <param name="time">Current time.</param>
        /// <returns>Updated status.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus SetTime(DateTime time)
        {
            this.Input.Now = time;
            return this;
        }

        /// <summary>
        /// Set message.
        /// </summary>
        /// <param name="message">Message to output.</param>
        /// <returns>Updated status.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus Message(string message)
        {
            this.Output.Message = message;
            return this;
        }

        /// <summary>
        /// Set drive direction.
        /// </summary>
        /// <param name="direction">Drive direction.</param>
        /// <returns>Updated status.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus Drive(DriveDirection direction)
        {
            this.Output.Drive = direction;
            return this;
        }

        /// <summary>
        /// Set drive direction.
        /// </summary>
        /// <param name="heading">Heading change in radians per second.</param>
        /// <param name="forward">Forward speed in meters per second.</param>
        /// <param name="distance">Distance in meters.</param>
        /// <returns>Updated status.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus Drive(double heading, double forward, double distance)
        {
            return this.Drive(new DriveDirection(heading, forward, distance));
        }

        /// <summary>
        /// Start collecting metrics.
        /// </summary>
        /// <returns>Updated status.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus StartCollectingMetrics()
        {
            this.Output.StartCollectingMetrics = true;
            return this;
        }

        /// <summary>
        /// Signal landmark arrival.
        /// </summary>
        /// <returns>Updated status.</returns>
        public PathPlanningAndTrackingMetricsControllerStatus SignalLandmarkArrival()
        {            
            this.Output.SignalLandmarkArrival = true;
            return this;
        }

        /// <summary>
        /// Container for values coming from the outside world.
        /// </summary>
        public class StatusInput
        {
            /// <summary>
            /// Gets or sets the starting timestamp (since reset).
            /// </summary>
            public DateTime Start { get; set; }

            /// <summary>
            /// Gets or sets the current timestamp (commonly DateTime.Now).
            /// </summary>
            public DateTime Now { get; set; }

            /// <summary>
            /// Gets or sets a value indicating whether the battery is charging.
            /// </summary>
            public bool BatteryCharging { get; set; }

            /// <summary>
            /// Gets or sets the battery charge percentage (0.0 - 100.0).
            /// </summary>
            public double BatteryPercentage { get; set; }

            /// <summary>
            /// Gets or sets the number of ground truth samples taken.
            /// </summary>
            public int LocalizationGroundTruthSamples { get; set; }

            /// <summary>
            /// Gets or sets the latest localization relative pose value.
            /// </summary>
            public Pose2D LocRelativePose { get; set; }

            /// <summary>
            /// Gets or sets the previous localization relative pose value.
            /// </summary>
            public Pose2D PreviousLocRelativePose { get; set; }

            /// <summary>
            /// Gets or sets the current path tracking drive direction.
            /// </summary>
            public DriveDirection DriveDirection { get; set; }

            /// <summary>
            /// Gets or sets the currently reported metrics.
            /// </summary>
            public PathPlanningAndTrackingMetrics NavMetrics { get; set; }

            /// <summary>
            /// Gets elapsed time since a relevant transition.
            /// </summary>
            public TimeSpan Elapsed
            {
                get { return this.Now - this.Start; }
            }

            /// <summary>
            /// Gets or sets the distance travelled
            /// </summary>
            public double Distance { get; set; }

            /// <summary>
            /// Gets or sets the previous drive feedback state.
            /// Null indicates the value is uninitialized.
            /// </summary>
            public DriveFeedbackState PreviousDriveFeedbackState { get; set; }

            /// <summary>
            /// Gets a value indicating whether the ground truth measurement is valid.
            /// </summary>
            /// <param name="pose">The pose to test</param>
            /// <remarks>When no marker is seen, a pose of NaN values is returned.</remarks>
            /// <returns>Boolean indicating if the pose if a valid ground truth measurement</returns>
            public bool IsValidGroundTruthMeasurement(Pose2D pose)
            {
                bool valid =
                    !double.IsNaN(this.LocRelativePose.X) &&
                    !double.IsNaN(this.LocRelativePose.Y) &&
                    this.LocRelativePose.HasHeading &&
                    !double.IsNaN(this.LocRelativePose.Heading);

                return valid;
            }

            /// <summary>
            /// Determine whether we've seen enough ground truth measurements and that they agree within a threshold.
            /// </summary>
            /// <param name="minSamples">Minimum number of ground truth samples taken at each waypoint.</param>
            /// <param name="consensusThreshold">Threshold of agreement between ground truth samples.</param>
            /// <returns>A value indicating whether we have completed ground truth measurement.</returns>
            public bool GroundTruthMeasurementComplete(int minSamples, double consensusThreshold)
            {
                Func<double, double, bool> valuesAgree = (x, y) => (double.IsNaN(x) && double.IsNaN(y)) || Math.Abs(x - y) < consensusThreshold;
                Func<Pose2D, Pose2D, bool> posesAgree = (p, q) => valuesAgree(p.X, q.X) && valuesAgree(p.Y, q.Y) && valuesAgree(p.Heading, q.Heading);

                bool agree = posesAgree(this.LocRelativePose, this.PreviousLocRelativePose);

                if (!agree && this.LocalizationGroundTruthSamples >= minSamples)
                {
                    TraceOut.Info(TraceContexts.Nav, "Sequential ground truth poses do not agree!");
                }
                
                return (this.LocalizationGroundTruthSamples >= minSamples)
                    && agree 
                    && this.IsValidGroundTruthMeasurement(this.LocRelativePose) 
                    && this.IsValidGroundTruthMeasurement(this.PreviousLocRelativePose);
            }
        }

        /// <summary>
        /// Container for values affecting the outside world.
        /// </summary>
        public class StatusOutput
        {
            /// <summary>
            /// Special value used as waypoint ID to indicate undocking.
            /// </summary>
            public const int UndockingSpecialWaypointId = -1;

            /// <summary>
            /// Special value used as waypoint ID to indicate docking.
            /// </summary>
            public const int DockingSpecialWaypointId = -2;

            /// <summary>
            /// Gets or sets a value indicating whether metrics should be started now.
            /// </summary>
            public bool StartCollectingMetrics { get; set; }

            /// <summary>
            /// Gets or sets status message.
            /// </summary>
            public string Message { get; set; }

            /// <summary>
            /// Gets or sets drive command to be issued.
            /// </summary>
            public DriveDirection Drive { get; set; }

            /// <summary>
            /// Gets or sets the index for the current waypoint.
            /// </summary>
            /// <remarks>
            /// Generally the target index, except -1 means undocking, -2 means docking.
            /// </remarks>
            public int WaypointId { get; set; }

            /// <summary>
            /// Gets or sets tracking target to be set.
            /// </summary>
            public Pose2D? Target { get; set; }

            /// <summary>
            /// Gets or sets a value indicating whether to signal arrival at the current waypoint.
            /// </summary>
            public bool SignalLandmarkArrival { get; set; }
            
            /// <summary>
            /// Gets or sets a value indicating whether to update metrics status.
            /// </summary>
            public bool UpdateMetricsStatus { get; set; }

            /// <summary>
            /// Gets or sets a value indicating whether to record navigation metrics and number of docking attempts.
            /// </summary>
            public bool RecordNavMetrics { get; set; }

            /// <summary>
            /// Gets or sets a value indicating whether to record localization metrics and number of docking attempts.
            /// </summary>
            public bool RecordLocMetrics { get; set; }

            /// <summary>
            /// Gets or sets a value indicating whether to initiate a turret scan.
            /// </summary>
            public bool InitiateTurretScan { get; set; }

            /// <summary>
            /// Gets or sets the current metrics.
            /// </summary>
            public PathPlanningAndTrackingMetrics NavMetrics { get; set; }

            /// <summary>
            /// Gets or sets the current localization relative pose metrics.
            /// </summary>
            public Pose2D LocRelativePose { get; set; }

            /// <summary>
            /// Gets or sets the number of docking attempts made this run.
            /// </summary>
            public int DockingAttempts { get; set; }

            /// <summary>
            /// Gets or sets a value indicating whether the metrics run was aborted.
            /// </summary>
            public bool Aborted { get; set; }

            /// <summary>
            /// Reset commands after processing by host.
            /// </summary>
            public void Reset()
            {
                this.StartCollectingMetrics = false;
                this.Message = null;
                this.Drive = null;
                this.Target = null;
                this.UpdateMetricsStatus = false;
                this.SignalLandmarkArrival = false;
                this.RecordNavMetrics = false;
                this.RecordLocMetrics = false;
                this.InitiateTurretScan = false;
                this.NavMetrics = new PathPlanningAndTrackingMetrics(
                    double.NaN,
                    double.NaN,
                    TimeSpan.MinValue,
                    new Pose2D(double.NaN, double.NaN, double.NaN),
                    double.NaN,
                    double.NaN,
                    double.NaN,
                    double.NaN);
                this.LocRelativePose = new Pose2D(double.NaN, double.NaN, double.NaN);
            }
        }
    }
}
